module util_fdm
    real(8), pointer, dimension(:) :: q
    real(8), pointer, dimension(:,:) :: hphiorg
    integer, private :: lcmfdacc
    
contains
subroutine evaluateDtilde
    implicit none
    
    include "xsec.FI"
    include "pspec.FI"
    include "arrays.FI"
    
    integer i, j, k, g
    real(8)    betaL, betaR, compL, compR
    
    do g= 1, ng    
        do i=1, totmesh
            ! for left boundary
            if (i.eq.1) then 
                compR = complist(i)
                betaR = Xdif(compR, g)/h(i)
                Dtilde_f(1, g) = 2*albedo(1)*betaR/(albedo(1)+2*betaR)
                compL = complist(i)
                compR = complist(i+1)
                betaL = Xdif(compL, g)/h(i)
                betaR = Xdif(compR, g)/h(i+1)
                Dtilde_f(2, g) = 2*betaL*betaR/(betaL+betaR)
            elseif(i.eq.totmesh) then
                compL = complist(i-1)
                compR = complist(i)
                betaL = Xdif(compL, g)/h(i-1)
                betaR = Xdif(compR, g)/h(i)
                Dtilde_f(2*totmesh-1, g) = 2*betaL*betaR/(betaL+betaR)
                compL = complist(i)
                betaL = Xdif(compL, g)/h(i)
                Dtilde_f(2*totmesh, g) = 2*albedo(2)*betaL/(albedo(2)+2*betaL)
            else
                compL = complist(i-1)
                compR = complist(i)
                betaL = Xdif(compL, g)/h(i-1)
                betaR = Xdif(compR, g)/h(i)
                Dtilde_f(2*i-1, g) = 2*betaL*betaR/(betaL+betaR)
                compL = complist(i)
                compR = complist(i+1)
                betaL = Xdif(compL, g)/h(i-1)
                betaR = Xdif(compR, g)/h(i)
                Dtilde_f(2*i , g) = 2*betaL*betaR/(betaL+betaR)
            end if
        end do
    end do
end subroutine
    
!subroutine evaluateDhat
!    implicit none
!    
!    include "xsec.FI"
!    include "pspec.FI"
!    include "arrays.FI"
!    
!    integer i, j, k, g
!    
!    do g= 1, ng    
!        do i=1, nRegion
!            do j=1, nmesh(i)
!                ! for left
!                if (j.eq.1) then 
!                elseif(j.eq.nmesh(i)) then
!                end if
!            end do
!        end do
!    end do
!    
!end  subroutine

subroutine make_matrix_f
    use LU_tdiag
    implicit none    
    
    include "xsec.FI"
    include "pspec.FI"
    include "arrays.FI"
    integer i, j, k, l, g, left, right
    real(8) ::offsum
    
    do g=1, ng
        do i=1, totmesh
            left = (i-1)*2+1
            right = i*2
            mt_f(1, i, g) = -Dtilde_f(left, g)
            mt_f(3, i, g) = -Dtilde_f(right,g)
            mt_f(2, i, g) =  Dtilde_f(left, g) + Dtilde_f(right,g) + Xrmv(complist(i), g) * h(i)
        end do
        mt_f(1, 1, g) = 0.
        mt_f(3, totmesh, g) = 0.
    end do
!    if (g .eq. 1)
!        open(111, file = 'grid-snu.in', status='unknown')
!        open(112, file = 'Matrix-snu.in', status = 'unknown')
!        write(111,*) totmesh
!        do i=1, totmesh
!            write(111,*) 2, (neigh(i,j),j=1,num_neigh(i))
!        end do
!    
!    end if
    do g=1, ng
        call LU_factorize(totmesh, mt_f(:,:,g))
    end do
end subroutine
!
!subroutine powermethod_fdm 
!    use LU_tdiag
!    implicit none
!    include "xsec.FI"
!    include "pspec.FI"
!    include "arrays.FI"
!    integer i, j, g, gp, iter
!    real(8) torErr, err, k, upper, lower
!    
!    iter   = 1
!    err    = 1.
!    k      = 1.
!    ! make fission source
!    do i=1, totmesh
!        fis(i) = 0
!    enddo
!    do g=1, ng
!        do i=1, totmesh
!            fis(i) = fis(i) + Xnuf(complist(i), g) * phi_f(i, g) * h(i);
!        enddo
!    enddo
!    
!    do while (err.gt.ccpsi)
!        do g=1, ng
!            ! make total source
!            do i=1, totmesh
!                rhs(i) = Xchi(complist(i),g)*fis(i)/k
!                do gp = 1, ng
!                    if (gp.eq.g)  then 
!                        continue
!                    else
!                        rhs(i) = rhs(i) + XsctM(g, gp, complist(i)) * phi_f(i, gp) * h(i)
!                    end if 
!                end do
!            end do
!            
!            ! solve 
!            call solve_LU (totmesh, mt_f(:,:,g), rhs, phi_f(:,g))
!        end do
!            
!        ! make fission source from updated flux
!        fis_prv = fis
!        do i=1, totmesh
!            fis(i) = 0
!        enddo
!        do g=1, ng
!            do i=1, totmesh
!                fis(i) = fis(i) + Xnuf(complist(i), g) * phi_f(i, g) * h(i);
!            enddo
!        enddo
!        
!        ! evaluate k
!        upper = 0.
!        lower = 0.
!        do i=1, totmesh
!            upper = upper + fis(i)*fis(i)
!            lower = lower + fis(i)*fis_prv(i)
!        end do
!        k = k*upper/lower
!        
!        ! evaluate err 
!        err = 0.
!        lower = sqrt(upper)
!        upper = 0
!        do i=1, totmesh
!            upper = upper + (fis(i)- fis_prv(i))*(fis(i)- fis_prv(i))
!        end do
!        upper = sqrt(upper)
!        err = upper/lower
!        
!        write (*, '(2(i6,2x,e12.5,2x,e12.5))'), iter, k, err
!        iter = iter + 1
!
!    end do
!    
!end subroutine

subroutine makeCurrent
    use define
    implicit none
    include "pspec.FI"
    include "arrays.FI"
    include "xsec.FI"
    integer :: g, i, i_f, i_c
    
    ! surface current condense    
    do g=1, ng
        do i=1, totmesh
            if (i.eq.1) then
            jf(i*2-1, g) = Dtilde_f(i*2-1, g) * phi_f(i, g)
!            jf(i*2-1, g) = albedo(1) * phi_f(i, g)
            jf(i*2, g) = -Dtilde_f(i*2, g) * (phi_f(i+1, g) - phi_f(i, g))
            elseif(i.eq.totmesh) then
            jf(i*2-1, g) = -Dtilde_f(i*2-1, g) * (phi_f(i-1, g) - phi_f(i, g))
            jf(i*2, g) = Dtilde_f(i*2, g) * phi_f(i, g)
!            jf(i*2, g) = albedo(2) * phi_f(i, g)
            else
            jf(i*2-1, g) = -Dtilde_f(i*2-1, g) * (phi_f(i-1, g) - phi_f(i, g))
            jf(i*2, g) = -Dtilde_f(i*2, g) * (phi_f(i+1, g) - phi_f(i, g))
            end if 
        end do
    end do
end subroutine

subroutine makeFS
    implicit none
    include "pspec.FI"
    include "arrays.FI"
    include "xsec.FI"
    integer :: i, g
    psi_f =0
    do g=1, ng
        do i=1, totmesh
            psi_f(i) = psi_f(i) + Xnuf(complist(i), g) * phi_f(i, g) * h(i);
        enddo
    enddo
end subroutine

subroutine makeQ(g, k, q)
    implicit none
    include "pspec.FI"
    include "arrays.FI"
    include "xsec.FI"
    integer,intent(in) :: g
    real(8),intent(in) :: k
    real(8),intent(out),dimension(:) ::q
    integer :: i,  gp
    q =0
    do i=1, totmesh
        q(i) = Xchi(1,g)*psi_f(i)/k
        do gp = 1, ng
            if (gp.eq.g)  then 
                continue
            else
                q(i) = q(i) + XsctM(g, gp, complist(i))*phi_f(i,gp)*h(i)
            end if 
        end do
    end do
end subroutine
    
subroutine prepare_fdm
    implicit none
    include "pspec.FI"
    allocate(q(totmesh));
endsubroutine

subroutine fdm(initk)
    use define
    use lu_tdiag
    use allocs
    use powermethod
    implicit none
    include "xsec.FI"
    include "pspec.FI"
    include "arrays.FI"
    include "files.FI"
    real(8), intent(out) :: initk
    real(8) :: k, err, dr
    logical :: op
    integer :: iter, i, g
    real(8) :: mxp
#ifdef PHI2NDHRM
    real(8) :: k2nd
    real(8), dimension(totmesh, ng) :: phi2nd, phitemp
#endif
    
    
    call dmalloc(hphiorg, totm_cmfd, ng)
    op = FALSE

    call evaluateDtilde
    call make_matrix_f
    k=1.
!    call powermethodG(totmesh, 3, mt_f, phi_f, psi_f, makeFS, makeQ, solve_LU, ccpsi, k, err, dr, iter)

    lcmfdacc = .true.
!    call powermethodaa(totmesh, 3, mt_f, phi_f, psi_f, makeFS, makeQ, solve_LU, ccpsi, k, err, dr, iter)

    call makeFS  
    open(1004, file="fdmflux.txt", status='unknown')
    do g=1, ng
        write(1004,'(2000(e11.5,1x))') (phi_f(i, g),i=1,totmesh)
    end do
    close(1004)
    print *, ""
    write(*, '(f12.5, 1x, f12.5)'), k, dr
    write(hfiter, '(2(f12.5))'), k,dr
    initk = k
    
!    call test_1g(k)

!#ifdef PHI2NDHRM
!    phitemp = phi_f
!    call powermethod_2nd (totmesh, mt_f, makeFS, makeQ, ccpsi, phi_f, psi_f, k, err, dr, iter)
!    write(*, '(a, 2(f12.5))'), 'Second :', k, dr
!    write(hfiter, '(a, 2(f12.5))'), 'Second :', k,dr
!    phi2nd = phi_f
!    phi_f = phitemp
!    open(1004, file="fdm2ndflux.txt", status='unknown')
!    do g=1, ng
!        write(1004,'(2000(e11.5,1x))') (phi2nd(i, g),i=1,totmesh)
!    end do
!    close(1004)
!#endif

end subroutine

subroutine test_1g(k)
    implicit none
    include "pspec.FI"
    include "xsec.FI"
    include "arrays.FI"
    real(8), intent(in) :: k
    integer :: i, m, o, j
    real(8) :: leak(3), absr(3), src(3), val(3), tot(3)
    real(8) :: diff(300), diff2(150), jl, phim, phio, phis
    ! 1 group test
    call makeCurrent
    do i=1, totmesh
        leak(1) = jf(i*2-1,1)+jf(i*2,1)
        leak(2) = jf(i*2-1,2)+jf(i*2,2)
        leak(3) = leak(1) + leak(2)
        absr(1) = phi_f(i,1)*Xrmv(complist(i), 1)*h(i)
        absr(2) = phi_f(i,2)*Xrmv(complist(i), 2)*h(i)
        src (1) = 1._8/k*psi_f(i)
        src (2) = XsctM(2,1,complist(i))*phi_f(i,1)*h(i)
        src (3) = src(1)
        absr(3) = absr(1)+absr(2)-src(2)
        tot (1) = Xtot(complist(i),1)*phi_f(i,1)*h(i)
        tot (2) = Xtot(complist(i),2)*phi_f(i,2)*h(i)
        tot (3) = (tot(1)+tot(2))/(phi_f(i,1)+phi_f(i,2))/h(i)
        val (1) = (leak(1) + absr(1) - src(1))/phi_f(i,1)
        val (2) = (leak(2) + absr(2) - src(2))/phi_f(i,2)
        val (3) = (leak(3) + absr(3) - src(3))/(phi_f(i,1)+phi_f(i,2))
        diff2(i) = 1._8/3/tot(3)
!        if (i<150) diff(i) = -(jf(i*2,1)+jf(i*2,2))/(phi_f(i+1,1)+phi_f(i+1,2)-(phi_f(i,1)+phi_f(i,2)))*h(i)

        m=i
        if (i .eq. 1) then
            j= i*2-1
            jl = jf(j,1)+jf(j,2)
            phim = phi_f(m,1)+phi_f(m,2)
            phis = (jl)*albedo(1)
            diff(j) = -h(i)/2*jl/(phis-phim) 

            o=m+1
            j= i*2
            jl = jf(j,1)+jf(j,2)
            phim = phi_f(m,1)+phi_f(m,2)
            phio = phi_f(o,1)+phi_f(o,2)
            phis = ( Xdif(complist(m), 1)*phi_f(m,1) + Xdif(complist(o), 1)*phi_f(o,1))/(Xdif(complist(m), 1)+Xdif(complist(o), 1))
            phis = phis + ( Xdif(complist(m), 2)*phi_f(m,2) + Xdif(complist(o), 2)*phi_f(o,2))/(Xdif(complist(m), 2)+Xdif(complist(o), 2))
            diff(j) = -h(i)/2*jl/(phis-phim) 
        
        elseif (i .eq. totmesh) then
            o=m-1
            j= i*2-1
            jl = jf(j,1)+jf(j,2)
            phim = phi_f(m,1)+phi_f(m,2)
            phio = phi_f(o,1)+phi_f(o,2)
            phis = ( Xdif(complist(m), 1)*phi_f(m,1) + Xdif(complist(o), 1)*phi_f(o,1))/(Xdif(complist(m), 1)+Xdif(complist(o), 1))
            phis = phis + ( Xdif(complist(m), 2)*phi_f(m,2) + Xdif(complist(o), 2)*phi_f(o,2))/(Xdif(complist(m), 2)+Xdif(complist(o), 2))
            diff(j) = -h(i)/2*jl/(phis-phim) 
            
            j= i*2
            jl = jf(j,1)+jf(j,2)
            phim = phi_f(m,1)+phi_f(m,2)
            phis = (jl)*albedo(2)
            diff(j) = -h(i)/2*jl/(phis-phim) 
        else
            o=m-1
            j= i*2-1
            jl = jf(j,1)+jf(j,2)
            phim = phi_f(m,1)+phi_f(m,2)
            phio = phi_f(o,1)+phi_f(o,2)
            phis = ( Xdif(complist(m), 1)*phi_f(m,1) + Xdif(complist(o), 1)*phi_f(o,1))/(Xdif(complist(m), 1)+Xdif(complist(o), 1))
            phis = phis + ( Xdif(complist(m), 2)*phi_f(m,2) + Xdif(complist(o), 2)*phi_f(o,2))/(Xdif(complist(m), 2)+Xdif(complist(o), 2))
            diff(j) = -h(i)/2*jl/(phis-phim) 

            o=m+1
            j= i*2
            jl = jf(j,1)+jf(j,2)
            phim = phi_f(m,1)+phi_f(m,2)
            phio = phi_f(o,1)+phi_f(o,2)
            phis = ( Xdif(complist(m), 1)*phi_f(m,1) + Xdif(complist(o), 1)*phi_f(o,1))/(Xdif(complist(m), 1)+Xdif(complist(o), 1))
            phis = phis + ( Xdif(complist(m), 2)*phi_f(m,2) + Xdif(complist(o), 2)*phi_f(o,2))/(Xdif(complist(m), 2)+Xdif(complist(o), 2))
            diff(j) = -h(i)/2*jl/(phis-phim) 
        
        endif

    end do
end subroutine

subroutine prolongation(cmfdphi)
    include "xsec.FI"
    include "pspec.FI"
    include "arrays.FI"
    integer :: i, j, g
    real(8), pointer, dimension(:,:) :: cmfdphi
    REAL(8) :: NRM, NRMF
    NRM = SUM(hphiorg)/SUM(CMFDPHI)
    
    do g=1, ng
        do i=1, totmesh
            i_c = fi2co(i)
            NRMF =cmfdphi(i_c,g)/hphiorg(i_c,g)*NRM
            phi_f(i,g) = phi_f(i,g)*NRMF
        end do
    enddo
end subroutine


subroutine powermethodaa(m,n, mt, phif, psi, funcFS, funcQ, lsolver, ccpsi, k, err, dr, iter)
    use LU_tdiag
    use ifport
    use util_cmfd ,only: cmfd, phi
    use HMGZ, only :homogenize_cycle
    implicit none
    include "xsec.FI"
    include "files.FI"
    include "hmgz.FI"
    interface 
        subroutine funcFS
        end subroutine
        subroutine funcQ(g, k, q)
            integer, intent(in) :: g
            real(8), intent(in) :: k
            real(8), intent(out), dimension(:) :: q
        end subroutine
        subroutine lsolver(sm, mt, rhs, sol)
            integer, intent(in) :: sm
            real(8),dimension(3,sm), intent(in) :: mt
            real(8),dimension(sm), intent(in) :: rhs
            real(8),dimension(sm), intent(out) :: sol
        end subroutine
        
    end interface
    integer, intent(in) :: m,n
    real(8), dimension(3,m,ng), intent(in):: mt
    real(8), intent(in):: ccpsi
    
    real(8), dimension(m,ng), intent(inout):: phif
    real(8), dimension(m), intent(inout):: psi
    real(8), dimension(m):: psi_prv
    real(8), dimension(m):: q
    real(8), intent(inout):: k
    real(8), intent(out) ::  err, dr
    integer, intent(out) :: iter
    real(8) :: err_cmfd
    integer :: iter_cmfd
    integer i, j, g, gp
    real(8) torErr, upper, lower
    real(8) :: err_prv
    
    iter   = 0
    err    = 1.
    dr = 1.
    
    ! make fission source
    call funcFS
    do while (err.gt.1e-8_8)
        err_prv = err
        iter = iter + 1
        do g=1, ng
            ! make total source
            call funcQ(g,k,q)
            ! solve 
            call lsolver (m, mt(:,:,g), q, phif(:,g))
        end do
        
            
        ! make fission source from updated flux
        psi_prv = psi
        call funcFS
                
        ! evaluate k                                            
        upper = 0.
        lower = 0.
        do i=1, m                                                
            upper = upper + psi(i)*psi(i)
            lower = lower + psi(i)*psi_prv(i)
        end do
        k = k*upper/lower
        
!        ! CMFD acc. 
!        if (lcmfdacc .eq. .true.)then
!!        if (iter < 30) then
!        call makeCurrent
!        call homogenize_cycle
!        hphiorg = hphi
!        call cmfd(k, err_cmfd, iter_cmfd, .false., .false.)
!        call prolongation(phi)
!!        endif
!        endif
        
        ! evaluate err 
        err = 0.
        lower = sqrt(upper)
        upper = 0
        do i=1, m
            upper = upper + (psi(i)- psi_prv(i))*(psi(i)- psi_prv(i))
        end do
        upper = sqrt(upper)
        err = upper/lower
        call funcFS
        if (iter==300) exit

    end do
    dr = err/err_prv
end subroutine

end module
